/*
 * @Description: 奥比中336L相机接口
 * @Author: BiChunkai 321521004@qq.com
 * @Date: 2025-06-03 12:22:04
 * @FilePath: /wsMyROS2_TEST/orbDemo/orbInterface/orb336LCamera.cpp
 *
 * Copyright (c) 2025 by 无锡捷普迅科技有限公司, All Rights Reserved.
 */
#include "orb336LCamera.h"

orb336LCamera::orb336LCamera() : align(OB_STREAM_COLOR)
{
    // Constructor implementation
    std::cout << "orb336LCamera constructor called." << std::endl;
    // Any necessary initialization code can be added here
    init();
    startCamera();
}

orb336LCamera::~orb336LCamera()
{
    // Destructor implementation
    std::cout << "orb336LCamera destructor called." << std::endl;
    // Any necessary cleanup code can be added here
    stopCamera();
}

bool orb336LCamera::getColorFrame(cv::Mat &rgbImage)
{
    auto frameset = pipe.waitForFrames();
    if (frameset == nullptr)
    {
        return false;
    }
    auto colorFrame = frameset->colorFrame();
    if (colorFrame != nullptr)
    {
        // 对齐处理
        auto newFrame = align.process(frameset);
        auto newFrameSet = newFrame->as<ob::FrameSet>();
        colorFrame = newFrameSet->colorFrame();
        cv::Mat colorMat;
        if (colorFrame->format() == OB_FORMAT_RGB888)
        {
            colorMat = cv::Mat(colorFrame->height(), colorFrame->width(),
                               CV_8UC3, (void *)colorFrame->data(),
                               cv::Mat::AUTO_STEP);
            cv::cvtColor(colorMat, colorMat, cv::COLOR_RGB2BGR);
        }

        rgbImage = colorMat.clone();
        return true;
    }
    else
    {
        return false;
    }
}

bool orb336LCamera::getDepthFrame(cv::Mat &rgbImage, cv::Mat &depthImage)
{
    auto frameset = pipe.waitForFrames();
    if (frameset == nullptr)
    {
        return false;
    }
    auto colorFrame = frameset->colorFrame();
    auto depthFrame = frameset->depthFrame();
    if (colorFrame != nullptr && depthFrame != nullptr)
    {
        // 对齐处理
        auto newFrame = align.process(frameset);
        auto newFrameSet = newFrame->as<ob::FrameSet>();
        colorFrame = newFrameSet->colorFrame();
        depthFrame = newFrameSet->depthFrame();
        cv::Mat colorMat, depthMat;

        if (colorFrame->format() == OB_FORMAT_RGB888)
        {
            colorMat = cv::Mat(colorFrame->height(), colorFrame->width(),
                               CV_8UC3, (void *)colorFrame->data(),
                               cv::Mat::AUTO_STEP);
            cv::cvtColor(colorMat, colorMat, cv::COLOR_RGB2BGR);
        }
        if (depthFrame->format() == OB_FORMAT_Y16)
        {
            depthMat = cv::Mat(depthFrame->height(), depthFrame->width(),
                               CV_16UC1, (void *)depthFrame->data(),
                               cv::Mat::AUTO_STEP);
        }
        rgbImage = colorMat.clone();
        depthImage = depthMat.clone();
        return true;
    }
    else
    {
        return false;
    }
}

void orb336LCamera::init()
{
    config = std::make_shared<ob::Config>();
    auto colorProfiles = pipe.getStreamProfileList(OB_SENSOR_COLOR);
    auto colorProfile = colorProfiles->getVideoStreamProfile(640, 480, OB_FORMAT_RGB888, 30);
    config->enableStream(colorProfile);

    auto depthProfiles = pipe.getStreamProfileList(OB_SENSOR_DEPTH);
    auto depthProfile = depthProfiles->getVideoStreamProfile(640, 480, OB_FORMAT_Y16, 30);
    config->enableStream(depthProfile);
}

void orb336LCamera::startCamera()
{
    pipe.start(config);
}
void orb336LCamera::stopCamera()
{
    pipe.stop();
}
